#include <arpa/inet.h>
#include <netinet/in.h>
#include <sys/socket.h>
#include <unistd.h>

#include <algorithm>
#include <chrono>
#include <csignal>
#include <cstring>
#include <iostream>
#include <map>
#include <string>
#include <thread>
#include <vector>

#include <unitree/robot/channel/channel_factory.hpp>
#include <unitree/robot/channel/channel_publisher.hpp>
#include <unitree/idl/hg/LowCmd_.hpp>
#include <unitree/idl/hg/HandCmd_.hpp>
// 陈凯旋加入，高层控制头文件
#include <unitree/robot/g1/loco/g1_loco_api.hpp>
#include <unitree/robot/g1/loco/g1_loco_client.hpp>

// G1 Dex3 机械手关节限制范围（来自g1_dex3_debug.cpp）
const double maxLimits_left[7] = {  1.05 ,  1.05  , 1.75 ,   0   ,  0    , 0     , 0   }; // 左手机械手最大电机值
const double minLimits_left[7] = { -1.05 , -0.724 ,   0  , -1.57 , -1.75 , -1.57  ,-1.75}; // 左手机械手最小电机值
const double maxLimits_right[7]= {  1.05 , 0.742  ,   0  ,  1.57 , 1.75  , 1.57  , 1.75}; // 右手机械手最大电机值
const double minLimits_right[7]= { -1.05 , -1.05  , -1.75,    0  ,  0    ,   0   ,0    }; // 右手机械手最小电机值

static const std::string kTopicArmSDK = "rt/arm_sdk";
static const std::string kTopicHandLeftSDK = "rt/dex3/left/cmd";
static const std::string kTopicHandRightSDK = "rt/dex3/right/cmd";

struct Options {
  std::string iface = "enp47s0"; // network interface name
  std::string listen_ip = "0.0.0.0";
  int listen_port = 18888;        // arm data port
  int hand_port = 18889;          // hand data port
  double rate_hz = 500.0;          // publish rate
  double max_vel = 0.8;            // rad/s joint speed limit
  double kp = 60.0;                // position gain
  double kd = 1.5;                 // damping gain
  bool dry_run = false;            // if true, do not publish
  bool enable_hand_control = true; // enable hand control
};

static bool g_running = true;
void signal_handler(int){ g_running = false; }

// minimal JSON array parser: expects {"names":[...],"rad":[...]} without escapes
struct UdpMsg {
  std::vector<std::string> names;
  std::vector<double> rad;
};

// hand data structure: expects {"hand_names":[...],"left_hand":[...],"right_hand":[...]}
struct HandMsg {
  std::vector<std::string> hand_names;
  std::vector<double> left_hand;
  std::vector<double> right_hand;
};

static bool parse_udp_json(const std::string& s, UdpMsg& out){
  auto find_array = [&](const std::string& key)->std::string{
    auto p = s.find("\""+key+"\""); if(p==std::string::npos) return {};
    p = s.find('[', p); if(p==std::string::npos) return {};
    int depth=0; size_t i=p; for(; i<s.size(); ++i){ if(s[i]=='[') depth++; else if(s[i]==']'){ depth--; if(depth==0){ i++; break; } } }
    if(i<=p) return {}; return s.substr(p, i-p);
  };
  auto names_arr = find_array("names");
  auto rad_arr = find_array("rad");
  if(names_arr.empty() || rad_arr.empty()) return false;
  auto split_items = [](const std::string& arr)->std::vector<std::string>{
    std::vector<std::string> items; std::string cur; int depth=0; bool in_string=false;
    for(size_t i=0;i<arr.size();++i){ char c=arr[i];
      if(c=='"') { in_string=!in_string; cur.push_back(c); continue; }
      if(!in_string && (c==',')) { if(cur.size()) { items.push_back(cur); cur.clear(); } continue; }
      if(!in_string && (c=='['||c==']')) { cur.push_back(c); continue; }
      cur.push_back(c);
    }
    if(cur.size()) items.push_back(cur);
    // remove brackets
    if(!items.empty()){
      if(items.front().size() && items.front()[0]=='[') items.front() = items.front().substr(1);
      if(items.back().size() && items.back().back()==']') items.back().pop_back();
    }
    return items;
  };
  auto names_items = split_items(names_arr);
  auto rad_items   = split_items(rad_arr);
  out.names.clear(); out.rad.clear();
  for(auto &it: names_items){
    std::string t; for(char c: it){ if(c!='"') t.push_back(c);} 
    if(t.size()) out.names.push_back(t);
  }
  for(auto &it: rad_items){
    try{ out.rad.push_back(std::stod(it)); }catch(...){ out.rad.push_back(0.0);} }
  return (!out.names.empty() && out.names.size()==out.rad.size());
}

// parse hand data JSON
static bool parse_hand_json(const std::string& s, HandMsg& out){
  auto find_array = [&](const std::string& key)->std::string{
    auto p = s.find("\""+key+"\""); if(p==std::string::npos) return {};
    p = s.find('[', p); if(p==std::string::npos) return {};
    int depth=0; size_t i=p; for(; i<s.size(); ++i){ if(s[i]=='[') depth++; else if(s[i]==']'){ depth--; if(depth==0){ i++; break; } } }
    if(i<=p) return {}; return s.substr(p, i-p);
  };
  
  auto names_arr = find_array("hand_names");
  auto left_arr = find_array("left_hand");
  auto right_arr = find_array("right_hand");
  
  if(names_arr.empty() || left_arr.empty() || right_arr.empty()) return false;
  
  auto split_items = [](const std::string& arr)->std::vector<std::string>{
    std::vector<std::string> items; std::string cur; int depth=0; bool in_string=false;
    for(size_t i=0;i<arr.size();++i){ char c=arr[i];
      if(c=='"') { in_string=!in_string; cur.push_back(c); continue; }
      if(!in_string && (c==',')) { if(cur.size()) { items.push_back(cur); cur.clear(); } continue; }
      if(!in_string && (c=='['||c==']')) { cur.push_back(c); continue; }
      cur.push_back(c);
    }
    if(cur.size()) items.push_back(cur);
    // remove brackets
    if(!items.empty()){
      if(items.front().size() && items.front()[0]=='[') items.front() = items.front().substr(1);
      if(items.back().size() && items.back().back()==']') items.back().pop_back();
    }
    return items;
  };
  
  auto names_items = split_items(names_arr);
  auto left_items = split_items(left_arr);
  auto right_items = split_items(right_arr);
  
  out.hand_names.clear(); out.left_hand.clear(); out.right_hand.clear();
  
  // parse names
  for(auto &it: names_items){
    std::string t; for(char c: it){ if(c!='"') t.push_back(c);} 
    if(t.size()) out.hand_names.push_back(t);
  }
  
  // parse left hand values
  for(auto &it: left_items){
    try{ out.left_hand.push_back(std::stod(it)); }catch(...){ out.left_hand.push_back(0.0);}
  }
  
  // parse right hand values
  for(auto &it: right_items){
    try{ out.right_hand.push_back(std::stod(it)); }catch(...){ out.right_hand.push_back(0.0);}
  }
  
  return (!out.hand_names.empty() && out.left_hand.size()==7 && out.right_hand.size()==7);
}

// Example mapping: fill from config later
struct Mapping {
  std::vector<int> toIndex;      // mapping from incoming order -> SDK motor index
  std::vector<double> sign;      // +1/-1
  std::vector<double> offset;    // rad offset
  std::vector<double> limitMin;  // rad
  std::vector<double> limitMax;  // rad
};

static double clamp(double x, double a, double b){ return std::max(a, std::min(b, x)); }

// 将Unity归一化数值（0-1）转换为G1弧度值
// 使用与g1_dex3_debug.cpp相同的线性插值算法
static double normalizeToRadians(double normalized_value, int joint_index, bool is_left_hand) {
  // 确保归一化值在0-1范围内
  normalized_value = clamp(normalized_value, 0.0, 1.0);
  
  const double* max_limits = is_left_hand ? maxLimits_left : maxLimits_right;
  const double* min_limits = is_left_hand ? minLimits_left : minLimits_right;
  
  // 使用线性插值：rad = min + normalized * (max - min)
  double radians = min_limits[joint_index] + normalized_value * (max_limits[joint_index] - min_limits[joint_index]);
  
  return radians;
}

int main(int argc, char** argv){
  signal(SIGINT,  signal_handler);
  signal(SIGTERM, signal_handler);

  Options opt;
  // parse minimal args: iface [arm_port] [hand_port]
  if(argc>=2) opt.iface = argv[1];
  if(argc>=3) opt.listen_port = std::atoi(argv[2]);
  if(argc>=4) opt.hand_port = std::atoi(argv[3]);

  // Init Unitree channel
  unitree::robot::ChannelFactory::Instance()->Init(0, opt.iface.c_str());

  // 陈凯旋 Init Loco Client，但先不启动计时线程
  unitree::robot::g1::LocoClient loco_client;
  loco_client.Init();
  loco_client.SetTimeout(10.f); // 设置一个合理的超时
  std::cout << "[bridge] LocoClient initialized." << std::endl;

  // Publisher & State subscriber
  unitree::robot::ChannelPublisherPtr<unitree_hg::msg::dds_::LowCmd_> arm_pub(
    new unitree::robot::ChannelPublisher<unitree_hg::msg::dds_::LowCmd_>(kTopicArmSDK));
  arm_pub->InitChannel();

  // Hand publishers
  unitree::robot::ChannelPublisherPtr<unitree_hg::msg::dds_::HandCmd_> left_hand_pub;
  unitree::robot::ChannelPublisherPtr<unitree_hg::msg::dds_::HandCmd_> right_hand_pub;
  
  if(opt.enable_hand_control){
    left_hand_pub.reset(new unitree::robot::ChannelPublisher<unitree_hg::msg::dds_::HandCmd_>(kTopicHandLeftSDK));
    right_hand_pub.reset(new unitree::robot::ChannelPublisher<unitree_hg::msg::dds_::HandCmd_>(kTopicHandRightSDK));
    left_hand_pub->InitChannel();
    right_hand_pub->InitChannel();
  }

  // 初期版本：仅发布，不订阅状态，避免 DDS Reader 初始化差异导致崩溃

  // UDP socket for arm data
  int sock = ::socket(AF_INET, SOCK_DGRAM, 0);
  sockaddr_in addr{}; addr.sin_family = AF_INET; addr.sin_port = htons(opt.listen_port); addr.sin_addr.s_addr = INADDR_ANY;
  if(::bind(sock, (sockaddr*)&addr, sizeof(addr))<0){ perror("bind arm socket"); return 1; }
  ::fcntl(sock, F_SETFL, O_NONBLOCK);

  // UDP socket for hand data
  int hand_sock = -1;
  if(opt.enable_hand_control){
    hand_sock = ::socket(AF_INET, SOCK_DGRAM, 0);
    sockaddr_in hand_addr{}; hand_addr.sin_family = AF_INET; hand_addr.sin_port = htons(opt.hand_port); hand_addr.sin_addr.s_addr = INADDR_ANY;
    if(::bind(hand_sock, (sockaddr*)&hand_addr, sizeof(hand_addr))<0){ perror("bind hand socket"); return 1; }
    ::fcntl(hand_sock, F_SETFL, O_NONBLOCK);
    std::cout << "[bridge] Hand socket created and bound to port " << opt.hand_port << std::endl;
  }

  // Mapping will be built dynamically from first UDP packet (by joint name)
  Mapping mapCfg;
  int N = 0; // will be set after first packet
  bool mapping_built = false;
  auto build_mapping = [&](const std::vector<std::string>& names){
    N = (int)names.size();
    mapCfg.toIndex.assign(N, -1);
    mapCfg.sign.assign(N, 1.0);
    mapCfg.offset.assign(N, 0.0);
    mapCfg.limitMin.assign(N, -2.5);
    mapCfg.limitMax.assign(N,  2.5);
    auto norm = [](std::string s){ for(auto &c:s) c = (char)tolower(c); return s; };
    for(int i=0;i<N;++i){
      std::string n = norm(names[i]);
      if(n=="waist_yaw_link")  { mapCfg.toIndex[i]=12; continue; }
      //if(n=="waist_roll_link") { mapCfg.toIndex[i]=13; continue; }
      //if(n=="waist_pitch_link"){ mapCfg.toIndex[i]=14; continue; }
      if(n=="waist_roll_link") { mapCfg.toIndex[i]=13; mapCfg.offset[i]=0.06; continue; }
      if(n=="torso_link"){ mapCfg.toIndex[i]=14; mapCfg.offset[i]=0.14; continue; }
      //link name == "torso_link" , joint name == "waist_pitch_joint".
      

      if(n=="left_shoulder_pitch_link") { mapCfg.toIndex[i]=15; continue; }
      if(n=="left_shoulder_roll_link")  { mapCfg.toIndex[i]=16; continue; }
      if(n=="left_shoulder_yaw_link")   { mapCfg.toIndex[i]=17; continue; }
      if(n=="left_elbow_link")          { mapCfg.toIndex[i]=18; continue; }
      if(n=="left_wrist_roll_link")     { mapCfg.toIndex[i]=19; continue; }
      if(n=="left_wrist_pitch_link")    { mapCfg.toIndex[i]=20; continue; }
      if(n=="left_wrist_yaw_link")      { mapCfg.toIndex[i]=21; continue; }

      if(n=="right_shoulder_pitch_link"){ mapCfg.toIndex[i]=22; continue; }
      if(n=="right_shoulder_roll_link") { mapCfg.toIndex[i]=23; continue; }
      if(n=="right_shoulder_yaw_link")  { mapCfg.toIndex[i]=24; continue; }
      if(n=="right_elbow_link")         { mapCfg.toIndex[i]=25; continue; }
      if(n=="right_wrist_roll_link")    { mapCfg.toIndex[i]=26; continue; }
      if(n=="right_wrist_pitch_link")   { mapCfg.toIndex[i]=27; continue; }
      if(n=="right_wrist_yaw_link")     { mapCfg.toIndex[i]=28; continue; }
      // other names (fingers etc.) remain -1 (ignored)
    }
    mapping_built = true;
  };

  // Control loop
  unitree_hg::msg::dds_::LowCmd_ cmd{};
  unitree_hg::msg::dds_::HandCmd_ left_hand_cmd{};
  unitree_hg::msg::dds_::HandCmd_ right_hand_cmd{};
  
  std::vector<double> q_des, q_curr;
  std::vector<double> left_hand_des(7, 0.0);
  std::vector<double> right_hand_des(7, 0.0);
  
  const double dt = 1.0 / opt.rate_hz;
  const double max_delta = opt.max_vel * dt;
  auto sleep_time = std::chrono::duration<double>(dt);
  bool has_udp = false;
  bool has_hand_udp = false;
  bool weight_up_done = false;
  bool printed_first = false;
  bool printed_mapping = false;
  bool printed_hand_first = false;

  std::cout << "Bridge listening on UDP arm:" << opt.listen_port;
  if(opt.enable_hand_control) std::cout << ", hand:" << opt.hand_port;
  std::cout << ", iface=" << opt.iface << " rate=" << opt.rate_hz << "Hz" << std::endl;

  while(g_running){
    // receive arm UDP
    char buf[65536]; sockaddr_in src{}; socklen_t slen=sizeof(src);
    int n = ::recvfrom(sock, buf, sizeof(buf)-1, 0, (sockaddr*)&src, &slen);
    if(n>0){ buf[n]=0; UdpMsg m; if(parse_udp_json(std::string(buf,n), m)){
        if(!printed_first){
          std::cout << "[bridge] recv first packet: names=" << m.names.size() << ", rad=" << m.rad.size() << std::endl;
          size_t show = std::min<size_t>(m.names.size(), 10);
          std::cout << "names[0.." << show-1 << "]: ";
          for(size_t i=0;i<show;i++){ std::cout << m.names[i] << (i+1<show?", ":"\n"); }
          show = std::min<size_t>(m.rad.size(), 10);
          std::cout << "rad[0.." << show-1 << "]: ";
          for(size_t i=0;i<show;i++){ std::cout << m.rad[i] << (i+1<show?", ":"\n"); }
          printed_first = true;
          if(!mapping_built){ build_mapping(m.names); q_des.assign(N,0.0); q_curr.assign(N,0.0); }
          if(mapping_built && !printed_mapping){
            std::cout << "[bridge] mapping (i: name -> sdkIdx):\n";
            for(int i=0;i<N;++i){
              int idx = mapCfg.toIndex[i];
              if(idx>=0) std::cout << "  ["<<i<<"] "<< m.names[i] << " -> " << idx << "\n";
            }
            printed_mapping = true;
          }
        }
        if(!mapping_built){ build_mapping(m.names); q_des.assign(N,0.0); q_curr.assign(N,0.0); }
        int M = std::min((int)m.rad.size(), N);
        for(int i=0;i<M;++i){
          int idx = mapCfg.toIndex[i]; if(idx<0 || idx>=N) continue;
          double x = mapCfg.sign[i] * (m.rad[i] - mapCfg.offset[i]);
          q_des[idx] = clamp(x, mapCfg.limitMin[i], mapCfg.limitMax[i]);
        }
        if(!has_udp){
          // 初次收到指令：将当前目标作为当前值，避免瞬时跳变
          for(int i=0;i<N;++i){ q_curr[i] = q_des[i]; }
          has_udp = true;
        //陈凯旋加入vim test: 
          std::thread motion_thread([&loco_client] {
              struct Action { int delay; float vx, vy, omega, duration; std::string desc; };

              std::vector<Action> sequence = {
                //{距离上一步结束等待多少秒， vx , vy , w , duration , "注释"},
                // //第一段
                //主
		//歌
		
                  {6,  0.11f,  0.0f,  0.0f,  3.0f, "Move Forward 0.2m/s"}, 
                  {4,  -0.26f, 0.0f, -0.1f,  3.0f, "Move Backward 0.2m/s"},
                  {3,  0.11f,  0.0f,  0.0f,  3.0f, "Move Forward 0.2m/s"}, 
                  {3,  -0.26f, 0.0f, -0.1f,  3.0f, "Move Backward 0.2m/s"},
                //预副歌
                  {10,  0.0f,  0.0f, -0.88f,  2.0f, "Move cycle 0.4m/s"},
                  {2,  0.0f,  0.0f,  0.8f,  2.0f, "Move cycle 0.4m/s"},  
                //副歌
                  {1,  0.11f,  0.0f,  0.0f,  3.0f, "Move Forward 0.2m/s"},
                  {3,  -0.26f, 0.0f,  0.0f,  3.0f, "Move Backward 0.2m/s"},
                  {3,  0.0f,  0.0f,  0.85f,  8.0f, "Move cycle 0.4m/s"},
                //第二段
                //主歌
                  {8,  0.11f,  0.0f,  0.0f,  3.0f, "Move Forward 0.2m/s"}, 
                  {4,  -0.26f, 0.0f, -0.1f,  3.0f, "Move Backward 0.2m/s"},
                  {3,  0.11f,  0.0f,  0.0f,  3.0f, "Move Forward 0.2m/s"}, 
                  {3,  -0.26f, 0.0f, -0.1f,  3.0f, "Move Backward 0.2m/s"},
                //预副歌
                  {10,  0.0f,  0.0f, -0.88f,  2.0f, "Move cycle 0.4m/s"},
                  {2,  0.0f,  0.0f,  0.8f,  2.0f, "Move cycle 0.4m/s"},               
                //副歌
                  {1,  0.11f,  0.0f,  0.0f,  3.0f, "Move Forward 0.2m/s"},
                  {3,  -0.26f, 0.0f,  0.0f,  3.0f, "Move Backward 0.2m/s"},
                  {3,  0.0f,  0.0f,  -0.85f,  8.0f, "Move cycle 0.4m/s"},
                //第三段
                //副歌
                  {5,  0.11f,  0.0f,  0.0f,  3.0f, "Move Forward 0.2m/s"}, 
                  {4,  -0.26f, 0.0f,  0.0f,  3.0f, "Move Backward 0.2m/s"},
                  {3,  0.0f,  0.0f,  0.82f,  8.0f, "Move cycle 0.4m/s"},
                  {5,  0.11f,  0.0f,  0.0f,  3.0f, "Move Forward 0.2m/s"}, 
                  {4,  -0.26f, 0.0f,  0.0f,  3.0f, "Move Backward 0.2m/s"},

                //预赛版：
                // //第一段
                // //主歌
                //   {6,  0.11f,  0.0f,  0.0f,  3.0f, "Move Forward 0.2m/s"}, 
                //   {4,  -0.26f, 0.0f,  0.0f,  3.0f, "Move Backward 0.2m/s"},
                //   {3,  0.11f,  0.0f,  0.0f,  3.0f, "Move Forward 0.2m/s"}, 
                //   {3,  -0.26f, 0.0f,  0.0f,  3.0f, "Move Backward 0.2m/s"},
                // //预副歌
                //   {4,  0.0f,  0.0f,  0.82f,  8.0f, "Move cycle 0.4m/s"},
                //   {1,  0.11f,  0.0f,  0.0f,  3.0f, "Move Forward 0.2m/s"},
                // //副歌
                //   {7,  -0.26f, 0.0f,  0.0f,  3.0f, "Move Backward 0.2m/s"},
                //   {3,  0.0f,  0.0f,  -0.82f,  8.0f, "Move cycle 0.4m/s"},
                // //第二段
                // //主歌
                //   {8,  0.11f,  0.0f,  0.0f,  3.0f, "Move Forward 0.2m/s"}, 
                //   {4,  -0.26f, 0.0f,  0.0f,  3.0f, "Move Backward 0.2m/s"},
                //   {3,  0.11f,  0.0f,  0.0f,  3.0f, "Move Forward 0.2m/s"}, 
                //   {3,  -0.26f, 0.0f,  0.0f,  3.0f, "Move Backward 0.2m/s"},
                // //预副歌
                //   {4,  0.0f,  0.0f,  0.82f,  8.0f, "Move cycle 0.4m/s"},
                //   {1,  0.11f,  0.0f,  0.0f,  3.0f, "Move Forward 0.2m/s"},                  
                // //副歌
                //   {7,  -0.26f, 0.0f,  0.0f,  3.0f, "Move Backward 0.2m/s"},
                //   {3,  0.0f,  0.0f,  -0.82f,  8.0f, "Move cycle 0.4m/s"},
                // //第三段
                // //副歌
                //   {5,  0.11f,  0.0f,  0.0f,  3.0f, "Move Forward 0.2m/s"}, 
                //   {4,  -0.26f, 0.0f,  0.0f,  3.0f, "Move Backward 0.2m/s"},
                //   {3,  0.0f,  0.0f,  0.82f,  8.0f, "Move cycle 0.4m/s"},
                //   {5,  0.11f,  0.0f,  0.0f,  3.0f, "Move Forward 0.2m/s"}, 
                //   {4,  -0.26f, 0.0f,  0.0f,  3.0f, "Move Backward 0.2m/s"},
                  



              };
              
              const int delay_seconds = 8; // 统一在这里设置总等待时间
              for (const auto& action : sequence) {
                  // --- A. 执行动作前的延时 ---
                  if (action.delay > 0) {
                      std::cout << "[Motion Thread] Waiting for " << action.delay << "s before next action: '" << action.desc << "'" << std::endl;
                      for (int i = action.delay; i > 0; --i) {
                          std::cout << "[Motion Thread] Countdown: " << i << "s remaining...  \r" << std::flush;
                          std::this_thread::sleep_for(std::chrono::seconds(1));
                      }
                      std::cout << std::endl;
                  }
                  // --- B. 发送动作指令 ---
                  std::cout << "[Motion Thread] Executing action: '" << action.desc 
                            << "' for " << action.duration << "s..." << std::endl;
                  
                  loco_client.SetVelocity(action.vx, action.vy, action.omega, action.duration);
                  
                  // --- C. 等待动作完成 ---
                  // SetVelocity 是非阻塞的，我们必须在这里暂停线程来确保机器人有时间执行动作。
                  std::this_thread::sleep_for(std::chrono::duration<float>(action.duration));

                  std::cout << "[Motion Thread] Action '" << action.desc << "' finished." << std::endl;
              }

              std::cout << "[Motion Thread] Entire action sequence finished. This thread is now exiting." << std::endl;
          });
      motion_thread.detach();
        }
      }
    }

    // receive hand UDP
    if(opt.enable_hand_control && hand_sock >= 0){
      char hand_buf[65536]; sockaddr_in hand_src{}; socklen_t hand_slen=sizeof(hand_src);
      int hand_n = ::recvfrom(hand_sock, hand_buf, sizeof(hand_buf)-1, 0, (sockaddr*)&hand_src, &hand_slen);
      if(hand_n>0){ 
        hand_buf[hand_n]=0; 
        // 调试输出（每500次输出一次）
        static int hand_json_counter = 0;
        if(++hand_json_counter % 500 == 0){
          std::cout << "[bridge] Hand JSON: " << std::string(hand_buf, hand_n) << std::endl;
        }
        HandMsg hm; if(parse_hand_json(std::string(hand_buf,hand_n), hm)){
          if(!printed_hand_first){
            std::cout << "[bridge] recv first hand packet: left=" << hm.left_hand.size() << ", right=" << hm.right_hand.size() << std::endl;
            std::cout << "left_hand: ";
            for(size_t i=0;i<hm.left_hand.size();i++){ std::cout << hm.left_hand[i] << (i+1<hm.left_hand.size()?", ":"\n"); }
            std::cout << "right_hand: ";
            for(size_t i=0;i<hm.right_hand.size();i++){ std::cout << hm.right_hand[i] << (i+1<hm.right_hand.size()?", ":"\n"); }
            printed_hand_first = true;
          }
          
          // 将Unity归一化参数（0-1）转换为G1弧度值
          for(int i=0; i<7; i++){
            left_hand_des[i] = normalizeToRadians(hm.left_hand[i], i, true);   // 左手
            right_hand_des[i] = normalizeToRadians(hm.right_hand[i], i, false); // 右手
          }
          has_hand_udp = true;
        }
      }
    }

    // 若尚未收到任何UDP，不发布任何关节命令，避免清空机器人控制导致下垂
    if(!has_udp){
      std::this_thread::sleep_for(sleep_time);
      continue;
    }

    // 未订阅状态：使用上一次下发值迭代跟踪目标，做速度限幅
    for(int i=0;i<N;++i){
      double err = q_des[i] - q_curr[i];
      double step = clamp(err, -max_delta, max_delta);
      double next = q_curr[i] + step;
      //q_curr[i] = next;/////q_curr[i] = q_des[i]//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
      q_curr[i] = q_des[i];
    }

    // 仅对受控关节（映射后的 SDK 索引）写命令，避免影响其他关节
    for(int i=0;i<N;++i){
      int sdkIdx = mapCfg.toIndex[i];
      if(sdkIdx < 0) continue;
      cmd.motor_cmd().at(sdkIdx).q(q_curr[sdkIdx]);
      cmd.motor_cmd().at(sdkIdx).dq(0.0);
      cmd.motor_cmd().at(sdkIdx).kp(opt.kp);
      cmd.motor_cmd().at(sdkIdx).kd(opt.kd);
      cmd.motor_cmd().at(sdkIdx).tau(0.0);
    }

    // engage weight to arms (per example using kNotUsedJoint as weight)
    // 收到首包后再渐进上权，避免瞬时掉权
    // Engage all reserved weight slots defensively (29..34) to 1.0 after ramp
    int firstWeight = 29;
    int lastWeight  = 34;
    static double w = 0.0;
    if(!weight_up_done){ w = std::min(1.0, w + 0.02); if(w>=1.0) weight_up_done = true; }
    for(int wi=firstWeight; wi<=lastWeight; ++wi){
      if(wi < (int)cmd.motor_cmd().size()) cmd.motor_cmd().at(wi).q(w);//这段全部注释///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    }

    if(!opt.dry_run){
      arm_pub->Write(cmd);
      
      // 发布手掌命令
      if(opt.enable_hand_control && has_hand_udp){
        // 设置左手命令（按照例程方式）
        left_hand_cmd.motor_cmd().resize(7);
        for(int i=0; i<7; i++){
          // 按照例程的RIS_Mode_t方式设置mode
          uint8_t mode = 0;
          mode |= (i & 0x0F);           // 设置电机ID
          mode |= (0x01 & 0x07) << 4;   // 设置状态为0x01
          mode |= (0x00 & 0x01) << 7;   // 设置超时标志为0
          left_hand_cmd.motor_cmd()[i].mode(mode);
          left_hand_cmd.motor_cmd()[i].q(left_hand_des[i]);
          left_hand_cmd.motor_cmd()[i].dq(0.0);
          left_hand_cmd.motor_cmd()[i].kp(1.5);  // 使用例程的kp值
          left_hand_cmd.motor_cmd()[i].kd(0.1);  // 使用例程的kd值
          left_hand_cmd.motor_cmd()[i].tau(0.0);
        }
        left_hand_pub->Write(left_hand_cmd);
        
        // 设置右手命令（按照例程方式）
        right_hand_cmd.motor_cmd().resize(7);
        for(int i=0; i<7; i++){
          // 按照例程的RIS_Mode_t方式设置mode
          uint8_t mode = 0;
          mode |= (i & 0x0F);           // 设置电机ID
          mode |= (0x01 & 0x07) << 4;   // 设置状态为0x01
          mode |= (0x00 & 0x01) << 7;   // 设置超时标志为0
          right_hand_cmd.motor_cmd()[i].mode(mode);
          right_hand_cmd.motor_cmd()[i].q(right_hand_des[i]);
          right_hand_cmd.motor_cmd()[i].dq(0.0);
          right_hand_cmd.motor_cmd()[i].kp(1.5);  // 使用例程的kp值
          right_hand_cmd.motor_cmd()[i].kd(0.1);  // 使用例程的kd值
          right_hand_cmd.motor_cmd()[i].tau(0.0);
        }
        right_hand_pub->Write(right_hand_cmd);
        
        // 调试：确认手掌命令发布
        static int hand_pub_counter = 0;
        if(++hand_pub_counter % 1000 == 0){
          std::cout << "[bridge] Hand commands published to DDS topics: " << kTopicHandLeftSDK << " and " << kTopicHandRightSDK << std::endl;
        }
        
        // 调试输出（每500次输出一次）
        static int debug_counter = 0;
        if(++debug_counter % 500 == 0){
          std::cout << "[bridge] Publishing hand commands - Left: [";
          for(int i=0; i<7; i++){ 
            std::cout << left_hand_des[i] << (i<6?", ":"");
          }
          std::cout << "], Right: [";
          for(int i=0; i<7; i++){ 
            std::cout << right_hand_des[i] << (i<6?", ":"");
          }
          std::cout << "]" << std::endl;
          
          // 显示转换前后的对比（每1000次输出一次）
          static int conversion_debug_counter = 0;
          if(++conversion_debug_counter % 1000 == 0){
            std::cout << "[bridge] Conversion debug - Unity normalized -> G1 radians:" << std::endl;
            std::cout << "Left hand: ";
            for(int i=0; i<7; i++){
              double radians = left_hand_des[i];
              std::cout << "[" << i << "] " << radians << " ";
            }
            std::cout << std::endl;
            std::cout << "Right hand: ";
            for(int i=0; i<7; i++){
              double radians = right_hand_des[i];
              std::cout << "[" << i << "] " << radians << " ";
            }
            std::cout << std::endl;
          }
        }
      }
    }

    std::this_thread::sleep_for(sleep_time);
  }

  // graceful stop: ramp weight down to 0
  unitree_hg::msg::dds_::LowCmd_ stop_cmd{};
  const int steps = 200; // ~1s at 200Hz
  for(int i=0;i<steps;i++){
    double w = 1.0 - (double)i/steps;
    const int kNotUsedJoint = 29;
    if(kNotUsedJoint < (int)stop_cmd.motor_cmd().size()){
      stop_cmd.motor_cmd().at(kNotUsedJoint).q(w);
    }
    arm_pub->Write(stop_cmd);
    std::this_thread::sleep_for(std::chrono::duration<double>(1.0/opt.rate_hz));
  }

  close(sock);
  if(hand_sock >= 0) close(hand_sock);
  return 0;
}


